#include "HOG.h"

using namespace cv;

namespace HOG
{
# define HOG_ORIENTATIONS 9
# define HOG_CELL_SIZE 8
# define SLIDING_WINDOW_STEPS_IN_HOG_CELLS 1
# define SLIDING_WINDOW_WIDTH 8
# define SLIDING_WINDOW_HEIGHT 16

	/*! Takes a WorkImage and calculates all the HOG-cells.
	* Those cells are stored as a MAT with float values within the datatype
	* @param input_obj working image Image, which HOG-Features are to be calculated
	*/
	void extractHOGFeatures(WorkImage *input_obj)
	{
		Mat input_img = input_obj->scaled_img;
		//Reserving memory for the 1-Dimensional float array and writing all the values to it in the correct order, so VL_Feat can interpret it
		float* frame = (float*)malloc(input_img.rows * input_img.cols * input_img.channels() * sizeof(float));
		for(int y=0; y<input_img.rows; y++)	
			for(int x=0; x<input_img.cols; x++)	
				for(int i=0; i<input_img.channels(); i++)	
					frame[x + y*input_img.cols + i*input_img.cols*input_img.rows] = (float)input_img.at<Vec3b>(y,x)[i];

		//Reserve memory for the hog-array and analyze the float array. Those values get written into the hog-array
		VlHog* hog =  vl_hog_new(VlHogVariantUoctti, vl_size(HOG_ORIENTATIONS), VL_FALSE) ;
		vl_hog_put_image(hog, frame, vl_size(input_img.cols), vl_size(input_img.rows), vl_size(input_img.channels()), vl_size(HOG_CELL_SIZE));
		free(frame);

		vl_size hogWidth = vl_hog_get_width(hog);
		vl_size hogHeight = vl_hog_get_height(hog);
		vl_size hogDimenison = vl_hog_get_dimension(hog);

		float* hogArray = (float*)vl_malloc(hogWidth * hogHeight * hogDimenison * sizeof(float));
		vl_hog_extract(hog, hogArray);

		Mat feature_mat = convert_hog_array(hogArray, (int)hogDimenison, (int)HOG_ORIENTATIONS, (int)hogWidth, (int)hogHeight, (int)hogWidth, (int)hogHeight);

		vl_size glyphSize = vl_hog_get_glyph_size(hog);
		vl_size imageHeight = glyphSize * hogHeight;
		vl_size imageWidth = glyphSize * hogWidth;

		Mat renderd_feature_array = Mat::zeros((int)imageHeight, (int)imageWidth, CV_32FC1);
		vl_hog_render(hog, renderd_feature_array.ptr<float>(0), hogArray, hogWidth, hogHeight);

		vl_free(hogArray);
		vl_hog_delete(hog);

		input_obj->hog_features= feature_mat;
	}

	/*!
	* Extract one sliding window at position from the feature_mat
	* 
	* @param feature_mat HOG array
	* @param top left position position of the slidiwng window in HOG array
	* @param nocpR number of cells per row (image.cols / hog cell size)
	* @return matrix with windowSizeX times windowSizeY many rows. each row 
	* stores one cell descriptor, i.e. the descriptor at grid pos. (r,c) is put in
	* row (r x windowSizeX) + c
	*/
	Mat extractWindowFromHogArray(Mat feature_mat, int position, int nocpR)
	{
		Mat result = Mat::zeros(SLIDING_WINDOW_HEIGHT * SLIDING_WINDOW_WIDTH, feature_mat.cols, CV_32FC1);

		for(int y=0; y<SLIDING_WINDOW_HEIGHT; y++)
			for(int x=0; x<SLIDING_WINDOW_WIDTH; x++)
			{
				// cr is the hog cell position in feature_mat
				int cr = position + (y * nocpR) + x;
				for(int h=0; h<feature_mat.cols; h++)
					result.ptr<float>((y*SLIDING_WINDOW_WIDTH)+x)[h] = feature_mat.ptr<float>(cr)[h];
			}

			return result;
	}

	/*!
	* Creates all sliding windows from the working image and saves them
	* in the datatype
	* @param w_image working image
	*/
	void createSlidingWindows(WorkImage* w_image)
	{
		Mat feature_mat = w_image->hog_features;
		int nocpR = w_image->scaled_img.cols/ HOG_CELL_SIZE;

		for (int x=0; x < w_image->scaled_img.cols-SLIDING_WINDOW_WIDTH * HOG_CELL_SIZE; x+=HOG_CELL_SIZE*SLIDING_WINDOW_STEPS_IN_HOG_CELLS)
		{
			for (int y=0; y < w_image->scaled_img.rows-SLIDING_WINDOW_HEIGHT * HOG_CELL_SIZE; y+=HOG_CELL_SIZE*SLIDING_WINDOW_STEPS_IN_HOG_CELLS)
			{
				// width = sliding window size * origin window size / scaled window size
				int width = (SLIDING_WINDOW_WIDTH * HOG_CELL_SIZE)/ w_image->scale_factor;
				int height = (SLIDING_WINDOW_HEIGHT * HOG_CELL_SIZE)/ w_image->scale_factor;
				WindowedHOG* w_hog = new WindowedHOG();
				w_hog->windowed_hog = extractWindowFromHogArray(feature_mat, ((y/HOG_CELL_SIZE)*nocpR)+(x/HOG_CELL_SIZE), nocpR);
				w_hog->window_rect = Rect(x/ w_image->scale_factor, y/ w_image->scale_factor, width, height);
				w_image->addLowerElement(w_hog);
			}
		}
	}
}